#pragma once

#include "opencv2/opencv.hpp"
#include <string>
#include <Windows.h>
#include "slStruct.h"

#ifndef slLog
#define slLog Log::getInstance()
#endif

using namespace cv;

namespace SL
{
    void rectify(Mat &imageL, Mat &imageR, Intrinsic &intrinsicL, Intrinsic &intrinsicR, Extrinsic &extrinsic, Mat &rectifiedImageL, Mat &rectifiedImageR, Mat &projectionMatrix)
    {
        // Mat imageLRotaionMatrix = Mat::eye(3, 3, CV_64F);
        // Mat imageRRotationMatrix = Mat::eye(3, 3, CV_64F); // extrinsic.rotation; // Mat::eye(3, 3, CV_64F); //
        // std::cout << imageLRotaionMatrix << std::endl;
        // std::cout << imageRRotationMatrix << std::endl;
        // Mat imageLProjectionMatrix = Mat::zeros(3, 4, CV_32FC1);
        // Mat imageRProjectionMatrix = Mat::zeros(3, 4, CV_32FC1);
        // intrinsicL.matrix(Range(0, 3), Range(0, 3)).copyTo(imageLProjectionMatrix(Range(0, 3), Range(0, 3)));
        // intrinsicR.matrix(Range(0, 3), Range(0, 3)).copyTo(imageRProjectionMatrix(Range(0, 3), Range(0, 3)));
        // imageRProjectionMatrix = imageRProjectionMatrix * extrinsic.matrix;
        // std::cout << imageLProjectionMatrix << std::endl;
        // std::cout << imageRProjectionMatrix << std::endl;

        Mat R1, R2, P1, P2, Q;
        Rect validRoi[2];
        Size imageSize = imageL.size();
        Mat intrinsicLMatrix, intrinsicRMatrix, extrinsicRotation;
        intrinsicL.matrix.convertTo(intrinsicLMatrix, CV_64F);
        intrinsicR.matrix.convertTo(intrinsicRMatrix, CV_64F);
        extrinsic.rotation.convertTo(extrinsicRotation, CV_64F);
        Vec3d extrinsicTranslation;
        extrinsicTranslation[0] = static_cast<double>(extrinsic.translation[0]);
        extrinsicTranslation[1] = static_cast<double>(extrinsic.translation[1]);
        extrinsicTranslation[2] = static_cast<double>(extrinsic.translation[2]);
        stereoRectify(intrinsicRMatrix, intrinsicR.kc, intrinsicLMatrix, intrinsicL.kc, imageSize, extrinsicRotation, extrinsicTranslation, R1, R2, P1, P2, projectionMatrix, 0, 0.3, imageSize, &validRoi[0], &validRoi[1]);
        Mat mapLx, mapLy, mapRx, mapRy;
        initUndistortRectifyMap(intrinsicL.matrix, intrinsicL.kc, R2, P2, imageSize, CV_16SC2, mapLx, mapLy);
        initUndistortRectifyMap(intrinsicR.matrix, intrinsicR.kc, R1, P1, imageSize, CV_16SC2, mapRx, mapRy);

        // stereoRectify(intrinsicLMatrix, intrinsicL.kc, intrinsicRMatrix, intrinsicR.kc, imageSize, extrinsicRotation, extrinsicTranslation, R1, R2, P1, P2, projectionMatrix, 0, 0.3, imageSize, &validRoi[0], &validRoi[1]);

        // Mat mapLx, mapLy, mapRx, mapRy;
        // initUndistortRectifyMap(intrinsicL.matrix, intrinsicL.kc, R1, P1, imageSize, CV_16SC2, mapLx, mapLy);
        // initUndistortRectifyMap(intrinsicR.matrix, intrinsicR.kc, R2, P2, imageSize, CV_16SC2, mapRx, mapRy);

        remap(imageL, rectifiedImageL, mapLx, mapLy, INTER_LINEAR);
        remap(imageR, rectifiedImageR, mapRx, mapRy, INTER_LINEAR);

        // Mat mapLx,
        //     mapLy, mapRx, mapRy;
        // Size sizeL = imageL.size();
        // Size sizeR = imageL.size();
        // initUndistortRectifyMap(intrinsicL.matrix, intrinsicL.kc, imageLRotaionMatrix, imageLProjectionMatrix, sizeL, CV_32F, mapLx, mapLy);
        // initUndistortRectifyMap(intrinsicR.matrix, intrinsicR.kc, imageRRotationMatrix, imageRProjectionMatrix, sizeR, CV_32F, mapRx, mapRy);

        // remap(imageL, rectifiedImageL, mapLx, mapLy, INTER_LINEAR);
        // remap(imageR, rectifiedImageR, mapRx, mapRy, INTER_LINEAR);
    }
}